#include "ABS_key.h"
#include "control.h"
#include "display.h"
#include <string.h>
#include <stdlib.h>
#include "C_code_Log.h"
#include "globalTime.h"
#include "camera.h"
#include "usart_port.h"
#include "motor.h"
#include "./AutoPlanLineWay/autoLineWay.h"
#include "color.h"

static uint8_t game(const uint8_t runTable[]);

/// @brief 显示数组,无符号8位整型（外部不调用）
/// @param row 显示的y值
/// @param array 8位无符号整型数组
/// @param length 显示的长度
static void show_U8Array(int row, const uint8_t *array, int length) {
  uint8_t colNum_max = Display_lastCol / (3 * 6);  // 一行最多显示的数字个数
  // 遍历数组
  for (int i = 0; i < length; i++) {
    // 计算当前行和列
    int currentRow = row + i / colNum_max * Display_spaced;
    int currentCol = (i % colNum_max) * (3 * 8);

    // 在指定的行和列显示数字
    displayNum_len(array[i], currentCol, currentRow, 3);
  }
}
#define MAX_DEBUG_NUM 20
struct DebugInfo {
  const uint8_t* (*getWayByIndex)(uint8_t index);//通过索引在调试路线表中获取调试路线的函数指针
  uint8_t runMethodNum;//路线图中路线的条数
  int8_t tableIndex;//选择的路线的索引
  const uint8_t* runMethod;//选择的路线,指向runMethodTable中的某一条路线
  uint8_t debugNum;//调试点的个数
  int8_t runNodeIndex;//选择第一个调试的点的索引
  uint8_t showFlag;//显示标志,0:刷新显示,1:不刷新显示
};
/**
 * @brief 选择调试路线
 * 
 * @param key_Num 键值
 * @param info 调试信息结构体  
 * @return int -1:按了esc,1:按了ok,0:其他
 * @note 按了esc返回-1,按了ok返回1,不会干其他事情,只会改变info的值,其他事情由调用者处理
 * @note 选择路线，决定调试信息中tableIndex和runMethod的值
 * @note 调用者要交出屏幕的控制权,这个函数需要使用到屏幕的显示
 */
static int chooseRunTable(int key_Num, struct DebugInfo* info) {
    // 如果按下ESC键，返回-1表示退出
    if (key_Num == Key_ESC) {
        return -1;
    }
    // 如果按下OK键，返回1表示确认
    else if (key_Num == Key_OK) {
        info->runMethod = info->getWayByIndex(info->tableIndex);
        return 1;
    }
    // 如果按下UP键，减少表索引并更新显示标志
    else if (key_Num == Key_UP) {
        info->tableIndex--;
        info->showFlag = 0;
        // 如果索引小于0，循环到最后一条路线
        if (info->tableIndex < 0) {
            info->tableIndex = info->runMethodNum - 1;
        }
    }
    // 如果按下DOWN键，增加表索引并更新显示标志
    else if (key_Num == Key_Down) {
        info->tableIndex++;
        info->showFlag = 0;
        // 如果索引大于等于表的数量，循环到第一条路线
        if (info->tableIndex >= info->runMethodNum) {
            info->tableIndex = 0;
        }
    }
    // 如果显示标志为0，更新显示内容
    if (info->showFlag == 0) {
        displayClean();  // 清除显示
        displayStr("choose table", Display_firstCol, Display_firstRow, RED);  // 显示提示信息
        displayNum_len(info->tableIndex, Display_lastCol, Display_firstRow, 2);  // 显示当前表索引
        // 显示当前路线的内容
        show_U8Array(Display_secondRow, info->getWayByIndex(info->tableIndex), 
        strlen((const char*)info->getWayByIndex(info->tableIndex)));
        info->showFlag = 1;  // 更新显示标志
    }
    return 0;  // 返回0表示继续选择
}
/**
 * @brief 选择调试点的个数
 * 
 * @param key_Num 键值
 * @param info 调试信息结构体
 * @return int -1:按了esc,1:按了ok,0:其他
 * @note 这个函数会使用到屏幕的显示，但是不会清屏，使用前请确保屏幕已经清屏
 */
static int chooseDebugNum(int key_Num, struct DebugInfo* info){ 
    if(key_Num==Key_ESC){
      return -1;
    }
    else if(key_Num==Key_OK){
      //确保调试点的个数不超过最大调试点的个数和路线的长度
      if(info->debugNum>MAX_DEBUG_NUM||info->debugNum>strlen((const char*)info->runMethod)){
        info->debugNum=min(strlen((const char*)info->runMethod),MAX_DEBUG_NUM);
      }
      return 1;
    }
    // 如果按下DOWN键，减少调试点的个数
    else if(key_Num==Key_Down){
      info->debugNum--;
      info->showFlag=0;
      // 如果调试点的个数小于3，设置为3
      if(info->debugNum<3){
        info->debugNum=3;
      }
    }
    // 如果按下UP键，增加调试点的个数
    else if(key_Num==Key_UP){
      info->debugNum++;
      info->showFlag=0;
      //调试点的个数不能大于最大调试点的个数,也不能大于路线的长度
      if(info->debugNum>MAX_DEBUG_NUM||info->debugNum>strlen((const char*)info->runMethod)){
        info->debugNum=min(strlen((const char*)info->runMethod),MAX_DEBUG_NUM);
      }
    }
    if(info->showFlag==0){
      displayStr("choose debugNum", Display_firstCol, Display_firstRow, RED);
      displayNum(info->debugNum, Display_firstCol, Display_secondRow);
      info->showFlag=1;
    }
    return 0;
}
/**
 * @brief 选择调试路段(从哪个点开始调试)
 * 
 * @param key_Num 键值
 * @param info 调试信息结构体
 * @return int -1:按了esc,1:按了ok,0:其他
 * @note 设置runNodeIndex的值,会使用到屏幕的显示，会清屏
 * @note 选择调试点，决定调试信息中runNodeIndex的值
 */
static int chooseWay(int key_Num,struct DebugInfo* info){
    if(key_Num==Key_ESC){
      return -1;
    }
    else if(key_Num==Key_OK){
      return 1;
    }
    else if(key_Num==Key_Down){
      info->runNodeIndex++;
      // 如果大于路线的长度减去调试点的个数，就变成0，即可以达到的最开始的开始调试位置
      if (info->runNodeIndex >
          strlen((const char*)info->runMethod) - info->debugNum) {
        info->runNodeIndex = 0;
      }
      info->showFlag = 0;
    }
    else if(key_Num==Key_UP){
      info->runNodeIndex--; 
      // 如果小于0，就变成最后一个调试点减去调试点的个数，即可以达到的最后的开始调试位置
      if (info->runNodeIndex <0) {         
        info->runNodeIndex = strlen((const char *)info->runMethod) - info->debugNum;
      }
      info->showFlag = 0;
    }
    if(info->showFlag==0){
      displayClean();
      displayStr("choose node", Display_firstCol, Display_firstRow, RED);
      displayNum_len(info->runNodeIndex, Display_lastCol, Display_firstRow, 2);
      show_U8Array(Display_secondRow, info->runMethod + info->runNodeIndex, info->debugNum);
      info->showFlag=1;
    }
    return 0;
}
static const uint8_t* Array_getWayByIndex(uint8_t index){
  return runMethodTable[index];
}
/// @brief 路段调试函数
/// @param key_Num
/// @return
/// @note 重点程序，核心
/// @note 选择路线，选择调试点的个数，选择调试点，进行调试
/*My_StatusTypeDef Fun_debug(int key_Num) {
  static uint8_t flag =
      0;  // 0:选择路线，1:选择调试点的个数，2:选择调试点（摇号）3:进行调试
  static int8_t tableIndex = 0;       // 选择的调试路线
  static uint8_t debugNum = 4;        // 调试点的个数
  static uint8_t runTable[MAX_DEBUG_NUM+1] = {0};  // 选择的路线
  static int8_t runTableIndex = 0;    // 选择路线中的第一个调试点
  extern uint8_t runMethodNum;        // 路线的个数
  static uint8_t showFlag = 0;        // 0:刷新显示，1:不刷新显示
  if (flag == 0) {                    // 选择路线，决定tableIndex的值
    if (key_Num == Key_ESC) {
      flag = 0;
      tableIndex = 0;
      debugNum = 4;
      showFlag = 0;
      runTableIndex = 0;
      memset(runTable, 0, sizeof(runTable));  // 恢复默认值
      return MY_OK;                           // 确认退出
    } else if (key_Num == Key_OK) {           // 确认选择
      flag = 1;        // 进入 “选择调试点的个数”
      displayClean();  // 清屏，在这里顺便完成下一步的初始化
      displayStr("choose debugNum", Display_firstCol, Display_firstRow, RED);
    } else if (key_Num == Key_Down) {  // 选择路线减一
      tableIndex--;                    // 选择路线减一
      showFlag = 0;                    // 刷新显示
      if (tableIndex < 0) {
        tableIndex = runMethodNum - 1;  // 如果小于0，就回到最后一条路线
      }
    } else if (key_Num == Key_UP) {  // 选择路线加一
      tableIndex++;                  // 选择路线加一
      showFlag = 0;                  // 刷新显示
      if (tableIndex >= runMethodNum) {
        tableIndex = 0;  // 如果大于路线的个数，就回到第一条路线
      }
    }
    if (showFlag == 0) {  // 只有需要刷新显示时才刷新，减小时间开销
      displayClean();
      displayStr("choose table", Display_firstCol, Display_firstRow, RED);
      displayNum_len(tableIndex, Display_lastCol, Display_firstRow, 2);
      show_U8Array(
          Display_secondRow, runMethodTable[tableIndex],
          strlen((const char *)runMethodTable[tableIndex]));  // 显示路线内容
      showFlag = 1;
    }
  } else if (flag == 1) {  // 选择调试点的个数，决定debugNum的值
    if (key_Num == Key_ESC) {          // 退出
      showFlag = 0;                    // 选择路线的界面需要刷新
      debugNum = 4;                    // 调试点的个数回到4
      flag = 0;                        // 回到选择路线
    } else if (key_Num == Key_OK) {    // 确认选择
      showFlag = 0;                    // 选择调试点的界面需要刷新
      flag = 2;                        // 进入选择调试点
    } else if (key_Num == Key_Down) {  // 调试点的个数减一
      debugNum--;                      // 调试点的个数减一
      if (debugNum < 3) {              // 如果小于3，就回到3
        debugNum = 3;
      }
    } else if (key_Num == Key_UP) {
      debugNum++;
      if (debugNum > MAX_DEBUG_NUM ||
          debugNum >
              strlen(
                  (const char *)runMethodTable
                      [tableIndex])) {  // 如果大于MAX_DEBUG_NUM或者大于路线的长度，就回到MAX_DEBUG_NUM或者路线的长度
        debugNum = min(strlen((const char *)runMethodTable[tableIndex]), MAX_DEBUG_NUM);
      }
    }
    displayNum(debugNum, Display_firstCol,
               Display_secondRow);  // 实时显示调试点的个数
  } else if (flag == 2) {  // 选择具体调试哪些点，决定runTableIndex的值
    if (showFlag == 0) {
      displayClean();
      displayStr("choose node", Display_firstCol, Display_firstRow, RED);
      displayNum_len(runTableIndex, Display_lastCol, Display_firstRow, 2);
      show_U8Array(Display_thirdRow, runMethodTable[tableIndex] + runTableIndex,
                   debugNum);  // 显示调试点，个数是上面选择的
      showFlag = 1;
    }
    if (key_Num == Key_ESC) {
      showFlag = 0;
      flag = 1;
      runTableIndex = 0;
      displayClean();  // 清屏，在这里顺便完成下一步的初始化
      displayStr("choose debugNum", Display_firstCol, Display_firstRow, RED);
    } else if (key_Num == Key_OK) {
      for (int i = 0; i < debugNum; i++) {
        runTable[i] = runMethodTable
            [tableIndex]
            [i + runTableIndex];  // 装填进去，把选择的调试点复制到runTable中
      }
      runTable[debugNum] = 0;
      flag = 3;
    } else if (key_Num == Key_Down) {
      showFlag = 0;
      runTableIndex--;
      if (runTableIndex <
          0) {  // 如果小于0，就变成最后一个调试点减去调试点的个数，即可以达到的最后的开始调试位置
        runTableIndex =
            strlen((const char *)runMethodTable[tableIndex]) - debugNum;
      }
    } else if (key_Num == Key_UP) {
      showFlag = 0;
      runTableIndex++;
      // 如果大于路线的长度减去调试点的个数，就变成0，即可以达到的最开始的开始调试位置
      if (runTableIndex >
          strlen((const char *)runMethodTable[tableIndex]) - debugNum) {
        runTableIndex = 0;
      }
    }
  }  // 运行路线调试程序，只完成一小段路，所以不用重复初始化什么的
  else if (flag == 3) {
    if (game(runTable) == 1) {
      flag = 2;
      showFlag = 0;
    }
  }
  return MY_BUSY;
}*/
/// @brief 路段调试函数
/// @param key_Num
/// @return
/// @note 重点程序，核心
/// @note 选择路线，选择调试点的个数，选择调试点，进行调试
My_StatusTypeDef Fun_debug(int key_Num){
  static struct Info{
    struct DebugInfo* info;
    uint8_t runTable[MAX_DEBUG_NUM+1];  // 选择的路线
    enum{
      CHOOSE_RUN_TABLE,
      CHOOSE_DEBUG_NUM,
      CHOOSE_WAY,
      RUN,
      ERROR_NOWAY,//没有路线
    }flag;
  }* debugInfo;
  //初始化
  if(debugInfo==NULL){
    debugInfo=(struct Info*)malloc(sizeof(struct Info));
    //内存分配失败
    if(debugInfo==NULL){
      LOG_ERROR("内存分配失败");
      return MY_EXIT;
    }
    debugInfo->info=(struct DebugInfo*)malloc(sizeof(struct DebugInfo));
    //内存分配失败
    if(debugInfo->info==NULL){
      free(debugInfo);
      LOG_ERROR("内存分配失败");
      return MY_EXIT;
    }
    //debugInfo->info->runMethodTable=runMethodTable;
    debugInfo->info->getWayByIndex=Array_getWayByIndex;
    extern uint8_t runMethodNum;        // 路线的个数
    debugInfo->info->runMethodNum=runMethodNum;
    if(runMethodNum==0){
      debugInfo->flag=ERROR_NOWAY;
      return MY_EXIT;
    }
    debugInfo->info->tableIndex=0;
    //默认调试点的个数为4
    debugInfo->info->debugNum=4;
    debugInfo->info->runNodeIndex=0;
    debugInfo->info->showFlag=0;
    memset(debugInfo->runTable,0,sizeof(debugInfo->runTable));
    //进入选择路线模式
    debugInfo->flag=CHOOSE_RUN_TABLE;
  }
  //初始化完成
  else{
    //选择路线
    if(debugInfo->flag==CHOOSE_RUN_TABLE){
      int ret=chooseRunTable(key_Num,debugInfo->info);
      //按了esc,返回
      if(ret==-1){
        //释放内存
        free(debugInfo->info);
        free(debugInfo);
        debugInfo=NULL;
        return MY_OK;
      }
      //按了ok,进入选择调试点的个数模式
      else if(ret==1){
        debugInfo->flag=CHOOSE_DEBUG_NUM;
        displayClean();
        debugInfo->info->showFlag=0;
      }
    }
    //选择调试点的个数
    else if(debugInfo->flag==CHOOSE_DEBUG_NUM){
      int ret=chooseDebugNum(key_Num,debugInfo->info);
      //按了esc,返回上一个模式
      if(ret==-1){
        debugInfo->flag=CHOOSE_RUN_TABLE;
        debugInfo->info->showFlag=0;
        //返回时要把更改的值还原
        debugInfo->info->debugNum=4;
      }
      //按了ok,进入选择调试路段模式
      else if(ret==1){
        debugInfo->flag=CHOOSE_WAY;
        debugInfo->info->showFlag=0;
      }
    }
    //选择调试路段
    else if(debugInfo->flag==CHOOSE_WAY){
      int ret=chooseWay(key_Num,debugInfo->info);
      //按了esc,返回选择调试点的个数模式
      if(ret==-1){
        debugInfo->flag=CHOOSE_DEBUG_NUM;
        debugInfo->info->showFlag=0;
        displayClean();
        //返回时要把更改的值还原
        debugInfo->info->runNodeIndex=0;
      }
      //按了ok,进入RUN调试模式
      else if(ret==1){
        debugInfo->flag=RUN;
        debugInfo->info->showFlag=0;
        //装填进去，把选择的调试点复制到runTable中
        for(int i=0;i<debugInfo->info->debugNum;i++){
          debugInfo->runTable[i]=debugInfo->info->runMethod[i+debugInfo->info->runNodeIndex];
        }
        //结束,最后一个点位置为0
        debugInfo->runTable[debugInfo->info->debugNum]=0;
      }
    }
    //运行路线调试程序
    else if(debugInfo->flag==RUN){
      if (game(debugInfo->runTable) == 1) {
        debugInfo->flag = CHOOSE_WAY;
        debugInfo->info->showFlag = 0;
      }
    }
    else if(debugInfo->flag==ERROR_NOWAY){
      displayStr("no way",Display_firstCol,Display_firstRow,RED);
      if(key_Num==Key_ESC){
        free(debugInfo->info);
        free(debugInfo);
        debugInfo=NULL;
        return MY_OK;
      }
    }
  }
  return MY_BUSY;
}

//自动规划出来的路线的双向链表
struct AutoWayList{
  uint8_t *way;//指向一个路线的数组,数组的最后一个元素为0
  struct AutoWayList* next;
  struct AutoWayList* prev;
}*autoWay_head=NULL;
/**
 * @brief 添加自动规划的路线
 * 
 * @param way 要添加的路线,数组的最后一个元素为0
 * @return int 0:失败,1：成功
 */
static int autoWayListAdd(uint8_t* way){
  //分配节点内存
  struct AutoWayList* p=(struct AutoWayList*)malloc(sizeof(struct AutoWayList));
  if(p==NULL){
    LOG_ERROR("内存分配失败");
    return 0;
  }
  //复制路线
  int length=strlen((const char*)way);
  p->way=(uint8_t*)malloc(length+1);
  if(p->way==NULL){
    LOG_ERROR("内存分配失败");
    free(p);
    return 0;
  }
  memcpy(p->way,way,length+1);

  p->next=NULL;
  p->prev=NULL;
  if(autoWay_head==NULL){
    autoWay_head=p;
  }
  else{
    struct AutoWayList* q=autoWay_head;
    while(q->next!=NULL){
      q=q->next;
    }
    q->next=p;
    p->prev=q;
  }
  return 1;
}
/**
 * @brief 释放自动规划的路线
 * 
 */
static void autoWayListFree(){
  struct AutoWayList* p=autoWay_head;
  while(p!=NULL){
    struct AutoWayList* q=p->next;
    free(p->way);
    free(p);
    p=q;
  }
  autoWay_head=NULL;
}
/**
 * @brief 移除自动规划的路线
 * 
 * @param p 要移除的路线的链表节点
 */
static void autoWayListRemove(struct AutoWayList* p){
  if(p==NULL){
    return;
  }
  if(p==autoWay_head){
    autoWay_head=p->next;
    if(autoWay_head!=NULL){
      autoWay_head->prev=NULL;
    }
  }
  else{
    p->prev->next=p->next;
    if(p->next!=NULL){
      p->next->prev=p->prev;
    }
  }
  free(p->way);
  free(p);
}
/**
 * @brief 在自动规划的路线中查找路线
 * 
 * @param index 路线的索引
 * @return const uint8_t* 
 */
static const uint8_t* autoWayListGetWayByIndex(uint8_t index){
  const struct AutoWayList* p=autoWay_head;
  for(uint8_t i=0;i<index;i++){
    if(p==NULL){
      LOG_ERROR("索引超出范围");
      //返回第一个路线
      return runMethodTable[0];
    }
    p=p->next;
  }
  if(p==NULL){
    LOG_ERROR("索引超出范围");
    //返回第一个路线
    return runMethodTable[0];
  }
  return p->way;
}
/**
 * @brief 在菜单中添加自动规划的路线
 * 
 * @param key_Num 
 * @return My_StatusTypeDef 
 * @note 选择开始点,目标点,目标点的下一个点,在指定处按ESC键添加路线
 */
My_StatusTypeDef AutoWayAdd(int key_Num){
  static struct Info{
    int8_t nowNode;
    int8_t targetNode;
    int8_t targetNextNode;
    int8_t pointIndex;//“>”号的位置
    int8_t flag;//
  }*info=NULL;
  //初始化
  if(info==NULL){
    //Info结构体初始化
    info=(struct Info*)malloc(sizeof(struct Info));
    if(info==NULL){
      LOG_ERROR("内存分配失败");
      return MY_EXIT;
    }
    info->nowNode=0;
    info->targetNode=0;
    info->targetNextNode=0;
    info->pointIndex=0;
    info->flag=0;

    //界面初始化
    displayStr(" NowNode:",Display_firstCol,Display_firstRow,RED);
    displayNum_len(info->nowNode,Display_lastCol,Display_firstRow,2);
    displayStr(" TargetNode:",Display_firstCol,Display_secondRow,RED);
    displayNum_len(info->targetNode,Display_lastCol,Display_secondRow,2);
    displayStr(" NextNode:",Display_firstCol,Display_thirdRow,RED);
    displayNum_len(info->targetNextNode,Display_lastCol,Display_thirdRow,2);
    displayStr(" point to here and",Display_firstCol,Display_fourthRow,RED);
    displayStr("press ESC to add",Display_firstCol,Display_fifthRow,RED);
    displayStr(">",Display_firstCol,Display_firstRow,RED);
  }
  //正常运行
  else{
    //按下ESC键
    if(key_Num==Key_ESC){
      //如果>指向" point to here and",则添加路线
      if(info->pointIndex==3){
        uint8_t way[100];
        int ret = planLineWay(info->nowNode,info->targetNode,info->targetNextNode,way);
        if(ret==0){
          info->flag=1;
          LOG_ERROR("规划路线失败");
          displayStr("plan way failed",Display_firstCol,Display_fifthRow+Display_spaced,RED);
          displayStr("pressOkToContinue",Display_firstCol,Display_fifthRow,RED);
        }else{
          info->flag=1;
          int ret2 = autoWayListAdd(way);
          if(ret2==0){
            LOG_ERROR("添加路线失败,路线的内存分配失败");
            displayStr("add way failed",Display_firstCol,Display_fifthRow+Display_spaced,RED);
            displayStr("memory overflow",Display_firstCol,Display_fifthRow+Display_spaced*2,RED);  
            displayStr("pressOkToContinue",Display_firstCol,Display_fifthRow,RED);
          }
          displayStr("pressOkToContinue",Display_firstCol,Display_fifthRow,RED);
          displayStr("plan way success",Display_firstCol,Display_fifthRow+Display_spaced,RED);
          show_U8Array(Display_fifthRow+Display_spaced*2,way,strlen((const char*)way));
        }
        info->pointIndex=0;
      } else {
        // 释放内存
        free(info);
        info = NULL;
        return MY_OK;
      }
    }
    // 按下OK键
    else if (key_Num == Key_OK) {
      if (info->flag == 1) {
        displayClean();
        info->flag = 0;
        // 界面初始化
        displayStr(" NowNode:", Display_firstCol, Display_firstRow, RED);
        displayNum_len(info->nowNode, Display_lastCol,Display_firstRow,2);
        displayStr(" TargetNode:", Display_firstCol, Display_secondRow, RED);
        displayNum_len(info->targetNode, Display_lastCol,Display_secondRow,2);
        displayStr(" TargetNextNode:", Display_firstCol, Display_thirdRow, RED);
        displayNum_len(info->targetNextNode,Display_lastCol, Display_thirdRow,2);
        displayStr(" point to here and", Display_firstCol, Display_fourthRow,RED);
        displayStr("press ESC to add", Display_firstCol, Display_fifthRow, RED);
        displayStr(">", Display_firstCol, Display_firstRow, RED);
      } else {
        // 进入下一步
        displayStr(" ", Display_firstCol,Display_firstRow + info->pointIndex * Display_spaced, RED);
        info->pointIndex++;
        if (info->pointIndex > 3) {
          info->pointIndex = 0;
        }
        displayStr(">", Display_firstCol, Display_firstRow + info->pointIndex * Display_spaced, RED);
      }
    }
    // 按下UP键
    else if (key_Num == Key_UP && info->flag == 0) {
      //选择的是nowNode
      if(info->pointIndex==0){
        info->nowNode++;
        if(info->nowNode>50){
          info->nowNode=0;
        }
        displayNum_len(info->nowNode,Display_lastCol,Display_firstRow,2);
      }
      //选择的是targetNode
      else if(info->pointIndex==1){
        info->targetNode++;
        if(info->targetNode>NODE_NUM){
          info->targetNode=1;
        }
        displayNum_len(info->targetNode,Display_lastCol,Display_secondRow,2);
      }
      //选择的是targetNextNode
      else if(info->pointIndex==2){
        info->targetNextNode++;
        if(info->targetNextNode>NODE_NUM){
          info->targetNextNode=1;
        }
        displayNum_len(info->targetNextNode,Display_lastCol,Display_thirdRow,2);
      }
    }
    // 按下DOWN键
    else if (key_Num == Key_Down && info->flag == 0) {
      //选择的是nowNode
      if(info->pointIndex==0){
        info->nowNode--;
        if(info->nowNode<1){
          info->nowNode=NODE_NUM;
        }
        displayNum_len(info->nowNode,Display_lastCol,Display_firstRow,2);
      }
      //选择的是targetNode
      else if(info->pointIndex==1){
        info->targetNode--;
        if(info->targetNode<1){
          info->targetNode=NODE_NUM;
        }
        displayNum_len(info->targetNode,Display_lastCol,Display_secondRow,2);
      }
      //选择的是targetNextNode
      else if(info->pointIndex==2){
        info->targetNextNode--;
        if(info->targetNextNode<1){
          info->targetNextNode=NODE_NUM;
        }
        displayNum_len(info->targetNextNode,Display_lastCol,Display_thirdRow,2);
      }
    }
  }
  return MY_BUSY;
}
/**
 * @brief 菜单中删除自动规划的路线
 * 
 * @param key_Num 
 * @return My_StatusTypeDef 
 * @note 选择路线，在指定处按ESC键删除路线
 */
My_StatusTypeDef AutoWayRemove(int key_Num){
  static struct Info{
    int8_t index;
    int8_t maxIndex;
    int8_t showFlag;//显示标志,0:刷新显示,1:不刷新显示
    int8_t pointIndex;//“>”号的位置
    struct AutoWayList* p;
  }*info=NULL;
  //初始化
  if(info==NULL){
    //Info结构体初始化
    info=(struct Info*)malloc(sizeof(struct Info));
    if(info==NULL){
      LOG_ERROR("内存分配失败");
      return MY_EXIT;
    }
    info->index=0;
    info->pointIndex=0;
    info->p=autoWay_head;
    info->showFlag=0;

    //自动规划的路线为空
    if(autoWay_head==NULL){
      info->maxIndex=0;
    } else {
      //计算最大索引
      int i = 1;
      struct AutoWayList* p=autoWay_head;
        while(p->next!=NULL){
          p=p->next;
          i++;
        }
      info->maxIndex=i;
    }    
    
    //界面初始化
    displayStr(" index:",Display_firstCol,Display_firstRow,RED);
    displayNum(info->index,Display_firstCol+Display_OneChar*7,Display_firstRow);
    displayStr(" pointToHereAnd",Display_firstCol,Display_secondRow,RED);
    displayStr("PressESC toRemove",Display_firstCol,Display_thirdRow,RED);
    displayStr(">",Display_firstCol,Display_firstRow,RED);
  }
  else{
    //按下ESC键
    if(key_Num==Key_ESC){
      //如果>指向" point to here and",则删除路线
      if(info->pointIndex==1&&info->maxIndex>0){
        autoWayListRemove(info->p);
        info->pointIndex=0;
        info->showFlag=0;
        info->maxIndex--;
        info->p=autoWay_head;
        info->index=0;
      } else {
        // 释放内存
        free(info);
        info = NULL;
        return MY_OK;
      }
    }
    //按下OK键
    else if(key_Num==Key_OK){
      //进入下一步
      displayStr(" ",Display_firstCol,Display_firstRow+info->pointIndex*Display_spaced,RED);
      info->pointIndex++;
      if(info->pointIndex>1){
        info->pointIndex=0;
      }

      displayStr(">",Display_firstCol,Display_firstRow+info->pointIndex*Display_spaced,RED);
    }
    //按下UP键
    else if(key_Num==Key_Down&&info->maxIndex>0){
      //选择的是index
      if(info->pointIndex==0){
        info->index++;
        if(info->index>=info->maxIndex){
          info->index=0;
        }
        displayNum(info->index,Display_firstCol+Display_OneChar*7,Display_firstRow);
        if(info->p->next!=NULL){
          info->p=info->p->next;
        }
        else{
          info->p=autoWay_head;
        }
        info->showFlag=0;
      }
    }
    //按下DOWN键
    else if(key_Num==Key_UP){
      //选择的是index
      if(info->pointIndex==0&&info->maxIndex>0){
        info->index--;
        if(info->index<0){
          info->index=0;
        }
        displayNum(info->index,Display_firstCol+Display_OneChar*7,Display_firstRow);
        //找到对应的路线
        if(info->p->prev!=NULL){
          info->p=info->p->prev;
        }
        else{
          struct AutoWayList* p=autoWay_head;
          while(p->next!=NULL){
            p=p->next;
          }
          info->p=p;
        }
        info->showFlag=0;
      }
    }
    if(info->showFlag==0){
      info->showFlag=1;
      //避免重复显示，先把显示的内容清空，清3行      
      displayCleanLine(Display_fourthRow);
      displayCleanLine(Display_fifthRow);
      displayCleanLine(Display_fifthRow+Display_spaced);
      if(info->maxIndex>0){
      // 显示路线
      show_U8Array(Display_fourthRow, info->p->way,
                   strlen((const char*)info->p->way));
      }
      else{
        displayStr("NO way to delete",Display_firstCol,Display_fourthRow,RED);
      }
    }
  }
  return MY_BUSY;
}
/**
 * @brief 自动规划的路线调试
 * 
 * @param key_Num 
 * @return My_StatusTypeDef 
 */
My_StatusTypeDef Fun_AutoWayDebug(int key_Num){
  static struct Info {
    struct DebugInfo* info;
    uint8_t runTable[MAX_DEBUG_NUM + 1];  // 选择的路线
    enum {
      CHOOSE_RUN_TABLE,
      CHOOSE_DEBUG_NUM,
      CHOOSE_WAY,
      RUN,
      ERROR_NOWAY,  // 没有路线
    } flag;
  }* debugInfo;
  // 初始化
  if (debugInfo == NULL) {
    debugInfo = (struct Info*)malloc(sizeof(struct Info));
    if (debugInfo == NULL) {
      LOG_ERROR("内存分配失败");
      return MY_EXIT;
    }
    debugInfo->info = (struct DebugInfo*)malloc(sizeof(struct DebugInfo));
    if (debugInfo->info == NULL) {
      free(debugInfo);
      LOG_ERROR("内存分配失败");
      return MY_EXIT;
    }
    debugInfo->info->getWayByIndex = autoWayListGetWayByIndex;
    //计算最大索引
    if(autoWay_head==NULL){
      debugInfo->info->runMethodNum=0;
      debugInfo->flag = ERROR_NOWAY;
    } else {
      //计算最大索引
      int i = 1;
      struct AutoWayList* p=autoWay_head;
        while(p->next!=NULL){
          p=p->next;
          i++;
        }
      debugInfo->info->runMethodNum=i;
      //其他初始化
      debugInfo->info->tableIndex = 0;
      // 默认调试点的个数为4
      debugInfo->info->debugNum = 4;
      debugInfo->info->runNodeIndex = 0;
      debugInfo->info->showFlag = 0;
      memset(debugInfo->runTable, 0, sizeof(debugInfo->runTable));
      // 进入选择路线模式
      debugInfo->flag = CHOOSE_RUN_TABLE;
    } 
  }
  else{
    //选择路线
    if (debugInfo->flag == CHOOSE_RUN_TABLE) {
      int ret = chooseRunTable(key_Num, debugInfo->info);
      // 按了esc,返回
      if (ret == -1) {
        // 释放内存
        free(debugInfo->info);
        free(debugInfo);
        debugInfo = NULL;
        return MY_OK;
      }
      // 按了ok,进入选择调试点的个数模式
      else if (ret == 1) {
        debugInfo->flag = CHOOSE_DEBUG_NUM;
        displayClean();
        debugInfo->info->showFlag = 0;
      }
    }
    // 选择调试点的个数
    else if (debugInfo->flag == CHOOSE_DEBUG_NUM) {
      int ret = chooseDebugNum(key_Num, debugInfo->info);
      // 按了esc,返回上一个模式
      if (ret == -1) {
        debugInfo->flag = CHOOSE_RUN_TABLE;
        debugInfo->info->showFlag = 0;
        // 返回时要把更改的值还原
        debugInfo->info->debugNum = 4;
      }
      // 按了ok,进入选择调试路段模式
      else if (ret == 1) {
        debugInfo->flag = CHOOSE_WAY;
        debugInfo->info->showFlag = 0;
      }
    }
    // 选择调试路段
    else if (debugInfo->flag == CHOOSE_WAY) {
      int ret = chooseWay(key_Num, debugInfo->info);
      // 按了esc,返回选择调试点的个数模式
      if (ret == -1) {
        debugInfo->flag = CHOOSE_DEBUG_NUM;
        debugInfo->info->showFlag = 0;
        displayClean();
        // 返回时要把更改的值还原
        debugInfo->info->runNodeIndex = 0;
      }
      // 按了ok,进入RUN调试模式
      else if (ret == 1) {
        debugInfo->flag = RUN;
        debugInfo->info->showFlag = 0;
        // 装填进去，把选择的调试点复制到runTable中
        for (int i = 0; i < debugInfo->info->debugNum; i++) {
          debugInfo->runTable[i] = debugInfo->info->runMethod[i + debugInfo->info->runNodeIndex];
        }
        // 结束,最后一个点位置为0
        debugInfo->runTable[debugInfo->info->debugNum] = 0;
      }
    }
    else if(debugInfo->flag==RUN){
      if (game(debugInfo->runTable) == 1) {
        debugInfo->flag = CHOOSE_WAY;
        debugInfo->info->showFlag = 0;
      }
    }
    else if(debugInfo->flag==ERROR_NOWAY){
      displayStr("no way",Display_firstCol,Display_firstRow,RED);
      if(key_Num==Key_ESC){
        free(debugInfo->info);
        free(debugInfo);
        debugInfo=NULL;
        return MY_OK;
      }
    }
  }
  return MY_BUSY;
}
// My_StatusTypeDef AutoWayTestConnect(int key_Num){
//   static int8_t runWayIndex=0;
//   static struct AutoWayList *p=NULL;
//   static int8_t flag;
//   //初始化
//   if(flag == 0){
//     p=autoWay_head;
//     if(p==NULL){
//       displayStr("NO Way",Display_firstCol,Display_firstRow,RED);
//       if(key_Num==Key_ESC){
//         return MY_OK;
//       }
//     }
//     flag = 1;
//     displayStr("WayIndex:",Display_firstCol,Display_firstRow,RED);
//     displayNum_len(runWayIndex,Display_lastCol,Display_firstRow,2);
//   }
//   else if(flag == 1){
//     if(key_Num==Key_ESC){
//       flag = 0;
//       return MY_OK;
//     }
//     else if(key_Num==Key_OK){
//       flag = 2;
//       displayClean();
//       displayStr("TestConnect",Display_firstCol,Display_firstRow,RED);

//     }
//     else if(key_Num==Key_UP){
//       if(p->next!=NULL){
//         p=p->next;
//         runWayIndex++;
//       }
//       else{
//         runWayIndex=0;
//         p=autoWay_head;
//       }
//       displayNum_len(runWayIndex,Display_lastCol,Display_firstRow,2);
//       displayCleanLine(Display_secondRow);
//       displayCleanLine(Display_thirdRow);
//       displayCleanLine(Display_fourthRow);
//       displayCleanLine(Display_fifthRow);
//       show_U8Array(Display_secondRow,p->way,strlen((const char*)p->way));
//     }
//     else if(key_Num==Key_Down){   
//       if(p->prev!=NULL){
//         p=p->prev;
//         runWayIndex--;
//       }
//       else{
//         struct AutoWayList* q=autoWay_head;
//         runWayIndex=0;
//         while(q->next!=NULL){
//           q=q->next;
//           runWayIndex++;
//         }
//         p=q;
//       }
//       displayNum_len(runWayIndex,Display_lastCol,Display_firstRow,2);
//       displayCleanLine(Display_secondRow);
//       displayCleanLine(Display_thirdRow);
//       displayCleanLine(Display_fourthRow);
//       displayCleanLine(Display_fifthRow);
//       show_U8Array(Display_secondRow,p->way,strlen((const char*)p->way));
//     }
//   }
//   else if(flag == 2){
    
//   }

// }
/**
 * @brief 路线运行
 * 
 * @param key_Num 
 * @return My_StatusTypeDef 
 */
My_StatusTypeDef Fun_Run(int key_Num) {
  static uint8_t flag = 0;
  static int8_t runTableIndex = 0;
  static uint8_t showFlag = 0;// 0:刷新显示，1:不刷新显示
  extern uint8_t runMethodNum;        // 路线的个数
  if (flag == 0) {
    if (key_Num == Key_ESC) {
      runTableIndex = 0;
      showFlag = 0;
      motorStop();
      return MY_OK;
    }
    else if (key_Num == Key_OK) {
      flag = 1;
      displayClean();
      displayStr("Run", Display_firstCol, Display_firstRow, RED);
    }
    else if (key_Num == Key_Down) {
      runTableIndex--;
      showFlag = 0;
      if (runTableIndex < 0) {
        runTableIndex = runMethodNum - 1;
      }
    }
    else if (key_Num == Key_UP) {
      runTableIndex++;
      showFlag = 0;
      if (runTableIndex >= runMethodNum) {
        runTableIndex = 0;
      }
    }
    if(showFlag == 0){
      displayClean();
      displayStr("choose table", Display_firstCol, Display_firstRow, RED);
      displayNum_len(runTableIndex, Display_lastCol, Display_firstRow, 2);
      show_U8Array(
          Display_secondRow, runMethodTable[runTableIndex],
          strlen((const char *)runMethodTable[runTableIndex]));  // 显示路线内容
      showFlag = 1;
    }
  }
  else if (flag == 1) {
    if (game(runMethodTable[runTableIndex]) == 1) {
      flag = 0;
      showFlag = 0;
    }
  }
  return MY_BUSY;
}
#include "color.h"
extern Color gl_doorColor[4];//全局红绿灯颜色，分别表示39，40，41，42号红绿灯的颜色
/**
 * @brief 返回第二次奔跑的路线图
 * 
 * @return uint8_t* 路线图，不要返回局部变量的地址
 * @note 可以根据红绿灯的颜色等来选择路线
 * @note 未实现
 */
static const uint8_t *Game_getRunMethodTable2(){
  uint8_t i;
  //根据绿灯选择第二段的路线
  for(i=0;i<4;i++){
    if(gl_doorColor[i] == COLOR_GREEN){
      return runMethodTable2[i];
    }
  }
  //保底基础路线
  return runMethodTable2[4]; 
}
void ShowFinial(uint8_t upright_scenic_spot,uint8_t platform_1_5,
                uint8_t platform_6,uint8_t platform_7,uint8_t platform_8);
void ShowFinialChinese(uint8_t upright_scenic_spot,uint8_t platform_1_5,
                uint8_t platform_6,uint8_t platform_7,uint8_t platform_8);
/**
 * @brief 比赛函数，跑两次
 * 
 * @param key_Num 
 * @return My_StatusTypeDef 
 */
My_StatusTypeDef Fun_GAME(int key_Num) {
  static uint8_t flag = 0;
  static int8_t runTableIndex = 0;
  static uint8_t showFlag = 0;      // 0:刷新显示，1:不刷新显示
  extern uint8_t runMethodNum;      // 路线的个数
  static const uint8_t *runTable2;  // 第二次奔跑的路线图
  if (flag == 0) {
    if (key_Num == Key_ESC) {
      runTableIndex = 0;
      showFlag = 0;
      motorStop();
      return MY_OK;
    }
    else if (key_Num == Key_OK) {
      flag = 1;
      displayClean();
      displayStr("First Run", Display_firstCol, Display_firstRow, RED);
    }
    else if (key_Num == Key_Down) {
      runTableIndex--;
      showFlag = 0;
      if (runTableIndex < 0) {
        runTableIndex = runMethodNum - 1;
      }
    }
    else if (key_Num == Key_UP) {
      runTableIndex++;
      showFlag = 0;
      if (runTableIndex >= runMethodNum) {
        runTableIndex = 0;
      }
    }
    if(showFlag == 0){
      displayClean();
      displayStr("choose table", Display_firstCol, Display_firstRow, RED);
      displayNum_len(runTableIndex, Display_lastCol, Display_firstRow, 2);
      show_U8Array(
          Display_secondRow, runMethodTable[runTableIndex],
          strlen((const char *)runMethodTable[runTableIndex]));  // 显示路线内容
      showFlag = 1;
    }
  }
  else if (flag == 1) {
    if (game(runMethodTable[runTableIndex]) == 1) {
      //第一次跑完了，切换路线，跑第二次
      displayStr("Second Run", Display_firstCol, Display_firstRow, RED);
      runTable2 = Game_getRunMethodTable2();//获取第二次的路线图
      flag = 2;
    }
  }
  else if (flag == 2) {
    if (game(runTable2) == 1) {//第二次跑完了
      flag = 0;
      displayClean();
      #if GAME_MODE == FIND_TREASURE
      showFlag = 0;
      #endif
      #if GAME_MODE == TRAVEL
      if(gl_doorColor[0] == COLOR_GREEN)  //39
      {
          ShowFinialChinese(8,10,1,2,2);
      }
      else if(gl_doorColor[1] == COLOR_GREEN)//40
      {
          ShowFinialChinese(8,10,1,2,2);
      }
      else if(gl_doorColor[2] == COLOR_GREEN)//41
      {
          ShowFinialChinese(8,10,2,2,2);
      }
      else if(gl_doorColor[3] == COLOR_GREEN)//42
      {
          ShowFinialChinese(8,10,1,2,2);
      }
      #endif
    }
  }

  return MY_BUSY;
}
/**
 * @brief 直接比赛函数，用于不进菜单直接比赛
 * 
 */
void game_direct(){
 static uint8_t flag;
  static const uint8_t*runTable2; //第二次奔跑的路线图
 if(flag==0){
  if(game(runMethodTable[0])==1){
   flag=1;
   runTable2=Game_getRunMethodTable2();//获取第二次的路线图
  }
 }
 if(flag == 1 && game(runTable2)==1){
  flag=3;
  #if GAME_MODE == TRAVEL
      displayClean();
      if(gl_doorColor[0] == COLOR_GREEN)  //39
      {
          ShowFinialChinese(8,10,1,2,2);
      }
      else if(gl_doorColor[1] == COLOR_GREEN)//40
      {
          ShowFinialChinese(8,10,1,2,2);
      }
      else if(gl_doorColor[2] == COLOR_GREEN)//41
      {
          ShowFinialChinese(8,10,2,2,2);
      }
      else if(gl_doorColor[3] == COLOR_GREEN)//42
      {
          ShowFinialChinese(8,10,1,2,2);
      }
      #endif
 }
}
/**
 * @brief 比赛函数
 * 
 * @param runTable 路线图 
 * @return uint8_t 1:比赛完成，0：比赛未完成
 * @note 只跑一次，且传进来的奔跑路线参数只在第一次调用时
 * （gameRunFlag=0的情况，game()函数返回1后，会重置gameRunFlag为0）会用来初始化，
 * 后面的调用都不会用来这个参数，所以如果在game()函数运行中在其他地方（如红绿灯）切换了路线，
 * 也是可以的，即使game()的调用者传入的路线是旧的，也会按照新切换的路线走，传入的参数是不会影响到函数的运行的
 */
static uint8_t game(const uint8_t runTable[]) {
  static uint8_t gameRunFlag = 0;  // 0:未初始化，1：初始化
  if (gameRunFlag == 0) {          // 控制台只进行一次初始化
    gameRunFlag = 1;
    controlCenterInit(&gl_controlCenter, mapInfo, runTable);
  } else if (gameRunFlag == 1) {  // 控制台初始化完成，进入运行状态
    if (gl_controlCenter.update == MY_READY) {  // 如果路段完成，就更新路段
      LOG_INFO("路段完成");
      controlCenterUpdate(&gl_controlCenter, mapInfo);
    } else if (gl_controlCenter.update ==
               MY_EXIT) {  // 如果路线完成（全部完成了），就退出
      LOG_INFO("路线完成");
      motorStop();
      gameRunFlag = 0;
      return 1;
    }
    if (gl_controlCenter.refresh() == true) {  
      speedUpHandle_Task(&gl_controlCenter);
      findLine_Task(&gl_controlCenter);
      roadBlocksHandle_Task(&gl_controlCenter);
      seekNodeHandle_Task(&gl_controlCenter);
      carParkHandle_Task(&gl_controlCenter);
      rotateHandle_Task(&gl_controlCenter);
    }
  }
	return 0;
}

/**
 * @brief 测试函数，测试自动规划的路线是否通，用在串口调试
 * 
 * @param index 路线在自动规划的路线中的索引
 */
void Test_AutoWayConnect(int index){
  //函数声明
  bool isFindInGlobalMap(int now, int next, int nNext);
  const uint8_t*way=autoWayListGetWayByIndex(index);
  int length=strlen((const char*)way);
  if(length<3){
    LOG_ERROR("路线长度小于3");
    return;
  }
  int i=0;
  for(i=0;i<length-2;i++){
    bool b = isFindInGlobalMap(way[i],way[i+1],way[i+2]);
    if(b==false){
      printf("路线不通，%d,%d,%d\r\n",way[i],way[i+1],way[i+2]);
    }
  }
}